#pragma once

#include "cartographer/rcm/imu/common.h"
#include "cartographer/rcm/imu/options.h"
#include <sophus/se3.hpp>

#define OFF_P 0
#define OFF_R 3
#define OFF_V 6
#define OFF_BA 9
#define OFF_BG 12

namespace cartographer
{
    const double eps = 1e-4;

    class ImuCalib
    {
    public:
        ImuCalib(const Sophus::SE3<double> &Tbc, const double &ng, const double &na, const double &ngw, const double &naw)
        {
            Set(Tbc, ng, na, ngw, naw);
        }

        ImuCalib(const ImuCalib &calib)
        {
            mbIsSet = calib.mbIsSet;
            // Sophus/Eigen parameters
            mTbc = calib.mTbc;
            mTcb = calib.mTcb;
            Cov = calib.Cov;
            CovWalk = calib.CovWalk;
        }

        ImuCalib()
        {
            mbIsSet = false;
        }

        void Set(const Sophus::SE3<double> &sophTbc, const double &ng, const double &na, const double &ngw, const double &naw)
        {
            mbIsSet = true;
            const double ng2 = ng * ng;
            const double na2 = na * na;
            const double ngw2 = ngw * ngw;
            const double naw2 = naw * naw;
            // Sophus/Eigen
            mTbc = sophTbc;
            mTcb = mTbc.inverse();
            Cov.diagonal() << ng2, ng2, ng2, na2, na2, na2;
            CovWalk.diagonal() << ngw2, ngw2, ngw2, naw2, naw2, naw2;
        }

    public:
        bool mbIsSet;
        // Sophus/Eigen implementation
        Sophus::SE3<double> mTcb;
        Sophus::SE3<double> mTbc;
        Eigen::DiagonalMatrix<double, 6> Cov, CovWalk;
    };

    inline ImuCalib GetImuCalib()
    {
        Eigen::Matrix3d rotation;
        rotation << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
        Eigen::Quaterniond quaternion(rotation);

        Eigen::Vector3d translation;
        translation << 0.0, 0.0, 0.0;
        Sophus::SE3<double> tf_imu_to_tracking = Sophus::SE3<double>(quaternion, translation);

        const auto &options = cartographer::ImuOptions::GetInstance();
        // double mImuPer = 1.0 / (double)options.frequency;
        // mImuPer = 0.001; //1.0 / (double) mImuFreq;
        double Ng = options.noise_gyro; // 陀螺仪噪声
        double Na = options.noise_acc;  // 加速度噪声
        double Ngw = options.walk_gyro; // 陀螺仪随机游走噪声
        double Naw = options.walk_acc;  // 加速度计随机游走噪声
        const double sf = std::sqrt(options.frequency);
        ImuCalib calib(tf_imu_to_tracking, Ng * sf, Na * sf, Ngw / sf, Naw / sf);
        return calib;
    }

    // Integration of 1 gyro measurement

    class IntegratedRotation
    {
    public:
        IntegratedRotation()
        {
        }

        IntegratedRotation(const Eigen::Vector3d &angular, const double time)
        {
            Eigen::Vector3d v = angular * time;
            const double d2 = v.squaredNorm();
            const double d = std::sqrt(d2);
            Eigen::Matrix3d W = Sophus::SO3d::hat(v);
            if (d < eps)
            {
                deltaR = Eigen::Matrix3d::Identity() + W;
                rightJ = Eigen::Matrix3d::Identity();
            }
            else
            {
                deltaR = Eigen::Matrix3d::Identity() + W * sin(d) / d + W * W * (1.0f - cos(d)) / d2;
                rightJ = Eigen::Matrix3d::Identity() - W * (1.0f - cos(d)) / d2 + W * W * (d - sin(d)) / (d2 * d);
            }
        }

    public:
        Eigen::Matrix3d deltaR;
        Eigen::Matrix3d rightJ; // right jacobian
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };

    /*
     * EIGEN_MAKE_ALIGNED_OPERATOR_NEW
     * Eigen库为了使用SSE加速，在内存上分配了128位的指针，涉及字节对齐问题，该问题在编译时不会报错，只在运行时报错
     * 该宏在new一个对象时会总是返回一个对齐的指针
     */
    class ImuPreintegrate
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        using Ptr = std::shared_ptr<ImuPreintegrate>;

        ImuPreintegrate()
        {
        }

        ImuPreintegrate(const ImuBias &biasInit)
        {
            const auto &calib = GetImuCalib();
            Nga = calib.Cov;
            NgaWalk = calib.CovWalk;
            Initialize(biasInit);
        }

        ImuPreintegrate(const ImuBias &biasInit, int64 poseTime)
            : poseTime_(poseTime)
        {
            const auto &calib = GetImuCalib();
            Nga = calib.Cov;
            NgaWalk = calib.CovWalk;
            Initialize(biasInit);
        }

        virtual ~ImuPreintegrate()
        {
        }

        void Reintegrate();

        void SetNewBias(const ImuBias &bias);

        void Initialize(const ImuBias &biasInit);

        void Update(const Eigen::Vector3d &acceleration, const Eigen::Vector3d &angVel, const double &dt);

        PVR GetDeltaPVR(const ImuBias &bias)
        {
            std::unique_lock<std::mutex> lock(mutex_);
            auto dba = bias.acc_ - biasOrg_.acc_;
            auto dbg = bias.gyro_ - biasOrg_.gyro_;
            PVR pvr;
            pvr.P = dP + JPg * dbg + JPa * dba;
            pvr.V = dV + JVg * dbg + JVa * dba;
            pvr.R = NormalizeRotation(dR * Sophus::SO3d::exp(JRg * dbg).matrix());
            return pvr;
        }

        PVR GetUpdatedDeltaPVR()
        {
            std::unique_lock<std::mutex> lock(mutex_);
            auto dba = biasDelta_.tail(3);
            auto dbg = biasDelta_.head(3);
            PVR pvr;
            pvr.P = dP + JPg * dbg + JPa * dba;
            pvr.V = dV + JVg * dbg + JVa * dba;
            pvr.R = NormalizeRotation(dR * Sophus::SO3d::exp(JRg * dbg).matrix().cast<double>());
            return pvr;
        }

        PVR GetOriginalDeltaPVR()
        {
            PVR pvr;
            pvr.P = dP;
            pvr.V = dV;
            pvr.R = dR;
            return pvr;
        }

        inline int64 GetPoseTime() const
        {
            return poseTime_;
        }

        bool hasIntegrated = false;

        int cnt = 0;

    public:
        double sumTime_;

        ImuBias biasOrg_;

        Eigen::Matrix3d dR;
        Eigen::Vector3d dV;
        Eigen::Vector3d dP;

        Eigen::Matrix3d JRg;
        Eigen::Matrix3d JVg;
        Eigen::Matrix3d JVa;
        Eigen::Matrix3d JPg;
        Eigen::Matrix3d JPa;
        Eigen::Matrix<double, 15, 15> jacobian_;

        Eigen::DiagonalMatrix<double, 6> Nga;
        Eigen::DiagonalMatrix<double, 6> NgaWalk;
        Eigen::Matrix<double, 15, 15> convariance_;

    private:
        void SetJacobianMatrix();

    private:
        int64 poseTime_ = 0;
        std::mutex mutex_;
        ImuBias biasNew_; // 新偏置
        std::vector<ImuMeasure> measures_;
        Eigen::Matrix<double, 6, 1> biasDelta_; // 新偏置与原偏置之差
    };
} // namespace cartographer